/*****************************************************************
**          大连理工大学 凌BUG战队&大连民族大学 C.ONE战队
**                **生死有命，富贵在天**
**    没有什么难题是一个通宵解决不了的，如果有那就整两个！
**---------------------------------------------------------------
** 项目名称：   dlmu-hero-robot
** 日    期：   2024-03-12
** 作    者：   MasterWang & Changed by TuxMonkey
**---------------------------------------------------------------
** 文 件 名：   remote_control.c
** 文件说明：   遥控器及键鼠指令解析和机器人控制指令转换
*****************************************************************/

/*---------------------INCLUDES----------------------*/
#include "remote_control.h"
#include "IMU.h"
#include "usart.h"

/*---------------------VARIABLES---------------------*/
//使用CCM数据RAM区(0x10003000-0x10003FFF 4KB)
//
RC_ctrl_t rc_ctrl __attribute__((at(0x10003000))) = {0};
uint8_t rc_update_flag __attribute__((at(0x100033F0))) = 0;

//
ROBOT_ctrl_t robot_ctrl __attribute__((at(0x10003400))) = {0};

//
AutoAiming_ctrl_t autoaiming_ctrl __attribute__((at(0x10003500))) = {0};
uint8_t autoaiming_update_flag __attribute__((at(0x100035F0))) = 0;


/*---------------------FUNCTIONS---------------------*/

/***********************************************************************
** 函 数 名： dbus_to_rc()
** 函数说明： 遥控器DBUS协议解析(在UART3中断调用)
**---------------------------------------------------------------------
** 输入参数： DBUS数据包地址,遥控器及键鼠指令变量地址
** 返回参数： 无
***********************************************************************/
void dbus_to_rc(uint8_t *dbus_buf, RC_ctrl_t *rc_ctrl)
{
	static uint8_t sw_left_last = RC_SW_MID;
	static uint8_t sw_right_last = RC_SW_DOWN;
	
	if((dbus_buf==NULL)||(rc_ctrl==NULL)){ return;}
	
	rc_ctrl->recv_cnt++;

	rc_ctrl->ch1 = (dbus_buf[0] | (dbus_buf[1] << 8)) & 0x07ff;
	rc_ctrl->ch2 = ((dbus_buf[1] >> 3) | (dbus_buf[2] << 5)) & 0x07ff;
	rc_ctrl->ch3 = ((dbus_buf[2] >> 6) | (dbus_buf[3] << 2) | (dbus_buf[4] << 10)) & 0x07ff;
	rc_ctrl->ch4 = ((dbus_buf[4] >> 1) | (dbus_buf[5] << 7)) & 0x07ff;
	rc_ctrl->sw_right = ((dbus_buf[5] >> 4) & 0x0003);
	rc_ctrl->sw_left = ((dbus_buf[5] >> 4) & 0x000C) >> 2;
	rc_ctrl->mouse.vx = dbus_buf[6] | (dbus_buf[7] << 8);
	rc_ctrl->mouse.vy = dbus_buf[8] | (dbus_buf[9] << 8);
	rc_ctrl->mouse.vz = dbus_buf[10] | (dbus_buf[11] << 8);
	rc_ctrl->mouse.press_l = dbus_buf[12];
	rc_ctrl->mouse.press_r = dbus_buf[13];
	rc_ctrl->keyboard.key_code = dbus_buf[14] | (dbus_buf[15] << 8);
	rc_ctrl->wheel = dbus_buf[16] | (dbus_buf[17] << 8);

	rc_ctrl->ch1 -= 1024;
	rc_ctrl->ch2 -= 1024;
	rc_ctrl->ch3 -= 1024;
	rc_ctrl->ch4 -= 1024;
	rc_ctrl->wheel -= 1024;
	
	if(rc_ctrl->sw_left!=sw_left_last)
	{
		if((sw_left_last==RC_SW_MID)&&(rc_ctrl->sw_left==RC_SW_UP)){rc_ctrl->sw_left_midtoup_flag=1;rc_ctrl->sw_left_uptomid_flag=0;rc_ctrl->sw_left_midtodown_flag=0;rc_ctrl->sw_left_downtomid_flag=0;}
		else if((sw_left_last==RC_SW_UP)&&(rc_ctrl->sw_left==RC_SW_MID)){rc_ctrl->sw_left_midtoup_flag=0;rc_ctrl->sw_left_uptomid_flag=1;rc_ctrl->sw_left_midtodown_flag=0;rc_ctrl->sw_left_downtomid_flag=0;}
		else if((sw_left_last==RC_SW_MID)&&(rc_ctrl->sw_left==RC_SW_DOWN)){rc_ctrl->sw_left_midtoup_flag=0;rc_ctrl->sw_left_uptomid_flag=0;rc_ctrl->sw_left_midtodown_flag=1;rc_ctrl->sw_left_downtomid_flag=0;}
		else if((sw_left_last==RC_SW_DOWN)&&(rc_ctrl->sw_left==RC_SW_MID)){rc_ctrl->sw_left_midtoup_flag=0;rc_ctrl->sw_left_uptomid_flag=0;rc_ctrl->sw_left_midtodown_flag=0;rc_ctrl->sw_left_downtomid_flag=1;}
		else {rc_ctrl->sw_left_midtoup_flag=0;rc_ctrl->sw_left_uptomid_flag=0;rc_ctrl->sw_left_midtodown_flag=0;rc_ctrl->sw_left_downtomid_flag=0;}
	}
	else {rc_ctrl->sw_left_midtoup_flag=0;rc_ctrl->sw_left_uptomid_flag=0;rc_ctrl->sw_left_midtodown_flag=0;rc_ctrl->sw_left_downtomid_flag=0;}
	
	if(rc_ctrl->sw_right!=sw_right_last)
	{
		if((sw_right_last==RC_SW_MID)&&(rc_ctrl->sw_right==RC_SW_UP)){rc_ctrl->sw_right_midtoup_flag=1;rc_ctrl->sw_right_uptomid_flag=0;rc_ctrl->sw_right_midtodown_flag=0;rc_ctrl->sw_right_downtomid_flag=0;}
		else if((sw_right_last==RC_SW_UP)&&(rc_ctrl->sw_right==RC_SW_MID)){rc_ctrl->sw_right_midtoup_flag=0;rc_ctrl->sw_right_uptomid_flag=1;rc_ctrl->sw_right_midtodown_flag=0;rc_ctrl->sw_right_downtomid_flag=0;}
		else if((sw_right_last==RC_SW_MID)&&(rc_ctrl->sw_right==RC_SW_DOWN)){rc_ctrl->sw_right_midtoup_flag=0;rc_ctrl->sw_right_uptomid_flag=0;rc_ctrl->sw_right_midtodown_flag=1;rc_ctrl->sw_right_downtomid_flag=0;}
		else if((sw_right_last==RC_SW_DOWN)&&(rc_ctrl->sw_right==RC_SW_MID)){rc_ctrl->sw_right_midtoup_flag=0;rc_ctrl->sw_right_uptomid_flag=0;rc_ctrl->sw_right_midtodown_flag=0;rc_ctrl->sw_right_downtomid_flag=1;}
		else {rc_ctrl->sw_right_midtoup_flag=0;rc_ctrl->sw_right_uptomid_flag=0;rc_ctrl->sw_right_midtodown_flag=0;rc_ctrl->sw_right_downtomid_flag=0;}
	}
	else {rc_ctrl->sw_right_midtoup_flag=0;rc_ctrl->sw_right_uptomid_flag=0;rc_ctrl->sw_right_midtodown_flag=0;rc_ctrl->sw_right_downtomid_flag=0;}
		
	sw_left_last = rc_ctrl->sw_left;
	sw_right_last = rc_ctrl->sw_right;
}

/***********************************************************************
** 函 数 名： rc_to_robot()
** 函数说明： 遥控器及键鼠指令向机器人控制指令转换(在UART3中断调用)
**---------------------------------------------------------------------
** 输入参数： 遥控器及键鼠指令变量地址,机器人控制指令变量地址
** 返回参数： 无
***********************************************************************/
void rc_to_robot(RC_ctrl_t *rc_ctrl, ROBOT_ctrl_t *robot_ctrl)
{
	static uint8_t key_gyro_rotation_flag = 0;
	static uint8_t key_shoot_open_flag = 0;
	
	uint8_t tx_buf[4]={0};
	
	//
	if(rc_ctrl->sw_right==RC_SW_DOWN) //右拨档下,保护模式,全部电机失能
	{
		robot_ctrl->movement_mode = MOTOR_DISABLE;
		
		key_gyro_rotation_flag = 0;
		key_shoot_open_flag = 0;
		robot_ctrl->bulletroom_open_flag=0;
		robot_ctrl->chassis_lock_flag=0;
		robot_ctrl->energy_aiming_flag=0;
	}
	else //电机使能,闭环控制
	{
		/**********************键鼠控制模式*******************/
		//
		if((rc_ctrl->keyboard.key_bit.R)||(rc_ctrl->sw_left_midtodown_flag==1)) {robot_ctrl->bulletroom_open_flag = 1;} //短按R/左拨档下，开弹舱盖
		else if(rc_ctrl->sw_left_downtomid_flag==1){robot_ctrl->bulletroom_open_flag = 0;} //左拨档中，关弹舱盖
		if(rc_ctrl->keyboard.key_bit.G){key_gyro_rotation_flag=1;} //短按G，开陀螺旋转
		if(rc_ctrl->keyboard.key_bit.V){robot_ctrl->chassis_lock_flag=1;} //短按V, 进入底盘旋转锁定
		if((rc_ctrl->keyboard.key_bit.CTRL)&&(rc_ctrl->keyboard.key_bit.C)) //短按CTRL+C,切换到击打能量机关模式
		{
			robot_ctrl->chassis_lock_flag=1;
			robot_ctrl->energy_aiming_flag=1;
			
			tx_buf[0]=0x55; tx_buf[1]=0x02; tx_buf[2]=0x01; tx_buf[3]=(tx_buf[1]+tx_buf[2]);	
			SendUART6DataPackage(tx_buf, 4);
		}
		
		if(rc_ctrl->keyboard.key_bit.F)//短按F, 恢复正常模式
		{
			key_gyro_rotation_flag=0; //关小陀螺
			robot_ctrl->bulletroom_open_flag=0; //关弹舱盖
			robot_ctrl->chassis_lock_flag=0; //关底盘旋转锁定
			robot_ctrl->energy_aiming_flag=0; //默认击打机器人模式
			
			tx_buf[0]=0x55; tx_buf[1]=0x02; tx_buf[2]=0x00; tx_buf[3]=(tx_buf[1]+tx_buf[2]);	
			SendUART6DataPackage(tx_buf, 4);
			
			robot_ctrl->yaw_angle_set_energy=0;
		}

		if(rc_ctrl->keyboard.key_bit.Q){key_shoot_open_flag=1;} //短按Q，开摩擦轮和红点激光
		else if(rc_ctrl->keyboard.key_bit.E){key_shoot_open_flag=0;} //短按E，关摩擦轮和红点激光
		
		//
		if(rc_ctrl->keyboard.key_bit.SHIFT){robot_ctrl->chassis_accelerate_flag=1;}else{robot_ctrl->chassis_accelerate_flag=0;} //常按SHIFT,底盘加速模式
		if(rc_ctrl->keyboard.key_bit.Z){robot_ctrl->ignore_shoot_heat_limit_flag=1;}else{robot_ctrl->ignore_shoot_heat_limit_flag=0;} //常按Z,无视枪口热量限制
		
		//自瞄模式
		if(robot_ctrl->energy_aiming_flag==1)//自瞄在电控方面主要是接收数据和处理，相对于自瞄精确度一个在视觉的图像处理，一个便是电控算法精度，现在普遍的控制算法便是PID算法取自经典控制理论，接下来若要改进精度可适当引入以最小二乘法为基础的卡尔曼滤波估测提高精度但是要进行一个处理时间与精度的取舍
		{
			if(rc_ctrl->mouse.press_r) //能量机关模式下，短按鼠标右键进入自瞄模式  不同的目标精度要求不同，关于PID在PID.c中找
			{
				robot_ctrl->auto_aiming_flag=1;
				
				if(rc_ctrl->keyboard.key_bit.C){tx_buf[0]=0x55; tx_buf[1]=0x03; tx_buf[2]=0x01; tx_buf[3]=(tx_buf[1]+tx_buf[2]);} //大能量机关
				else {tx_buf[0]=0x55; tx_buf[1]=0x03; tx_buf[2]=0x00; tx_buf[3]=(tx_buf[1]+tx_buf[2]);} //小能量机关
				SendUART6DataPackage(tx_buf, 4);
			} 
		}
		else
		{
			if(rc_ctrl->mouse.press_r){robot_ctrl->auto_aiming_flag=1;}else{robot_ctrl->auto_aiming_flag=0;} //常按鼠标右键,自瞄模式
		}

		//ui界面分为快速模式0x01、普通模式0x02、调试模式0x04和能量机关模式0x08
		if(robot_ctrl->energy_aiming_flag==1)
		{
			if((robot_ctrl->ui_show_mode&0x08)==0){robot_ctrl->ui_show_mode = 0x88;} //其他模式->能量机关模式
		}
		else if(robot_ctrl->energy_aiming_flag==0)
		{
			if((robot_ctrl->ui_show_mode&0x80)==0) //非初始化静态界面
			{
				if((robot_ctrl->ui_show_mode&0x08)!=0){robot_ctrl->ui_show_mode = 0x82;} //能量机关模式->普通模式
				else if(((robot_ctrl->ui_show_mode&0x02)!=0)&&(rc_ctrl->mouse.vz>0)){robot_ctrl->ui_show_mode=0x81;} //普通模式+前滚->快速模式
				else if(((robot_ctrl->ui_show_mode&0x02)!=0)&&(rc_ctrl->mouse.vz<0)){robot_ctrl->ui_show_mode=0x84;} //普通模式+后滚->调试模式
				else if(((robot_ctrl->ui_show_mode&0x01)!=0)&&(rc_ctrl->mouse.vz<0)){robot_ctrl->ui_show_mode=0x82;} //快速模式+后滚->普通模式
				else if(((robot_ctrl->ui_show_mode&0x04)!=0)&&(rc_ctrl->mouse.vz>0)){robot_ctrl->ui_show_mode=0x82;} //调试模式+前滚->普通模式
				else if(((robot_ctrl->ui_show_mode&0x04)!=0)&&(rc_ctrl->mouse.vz<0)){robot_ctrl->ui_show_mode=0x84;} //调试模式+后滚->刷新调试模式
			}
		}
		
		//
		if((rc_ctrl->keyboard.key_bit.CTRL)&&(rc_ctrl->keyboard.key_bit.Z)) //开录像
		{
			tx_buf[0]=0x55; tx_buf[1]=0x09; tx_buf[2]=0x01; tx_buf[3]=(tx_buf[1]+tx_buf[2]);	
			SendUART6DataPackage(tx_buf, 4);
		}
		else if((rc_ctrl->keyboard.key_bit.CTRL)&&(rc_ctrl->keyboard.key_bit.X)) //关录像
		{
			tx_buf[0]=0x55; tx_buf[1]=0x09; tx_buf[2]=0x00; tx_buf[3]=(tx_buf[1]+tx_buf[2]);
			SendUART6DataPackage(tx_buf, 4);
		}
		
		/**********************运动模式控制*******************/		
		//
		if((key_gyro_rotation_flag==1)||(rc_ctrl->sw_right==RC_SW_UP)) //右拨档上,底盘陀螺旋转模式
		{
			robot_ctrl->movement_mode = CHASSIS_GYRO_ROTATION;
		}
		else if(rc_ctrl->sw_right==RC_SW_MID) //右拨档中，底盘跟随云台模式
		{
			robot_ctrl->movement_mode = CHASSIS_FOLLOW_GIMBAL;
		}
		
		/**********************底盘移动控制*******************/
		//vx
		if(rc_ctrl->keyboard.key_bit.W) {robot_ctrl->vx_set += 100; }
		else if(rc_ctrl->keyboard.key_bit.S) {robot_ctrl->vx_set -= 100; }
		else {robot_ctrl->vx_set = rc_ctrl->ch4*5;}
		
		//vx_limit
		if(robot_ctrl->vx_set>CHASSIS_X_MAXSPEED){robot_ctrl->vx_set=CHASSIS_X_MAXSPEED;}
		else if(robot_ctrl->vx_set<CHASSIS_X_MINSPEED){robot_ctrl->vx_set=CHASSIS_X_MINSPEED;}
		
		//vy
		if(rc_ctrl->keyboard.key_bit.A){robot_ctrl->vy_set += 100; }
		else if(rc_ctrl->keyboard.key_bit.D){robot_ctrl->vy_set -= 100; }
		else{robot_ctrl->vy_set = -rc_ctrl->ch3*5;}
		
		//vy_limit
		if(robot_ctrl->vy_set>CHASSIS_Y_MAXSPEED){robot_ctrl->vy_set=CHASSIS_Y_MAXSPEED;}
		else if(robot_ctrl->vy_set<CHASSIS_Y_MINSPEED){robot_ctrl->vy_set=CHASSIS_Y_MINSPEED;}
		
		
		/**********************云台转动控制*******************/
		//yaw
		if(robot_ctrl->energy_aiming_flag==1)
		{
			if((robot_ctrl->auto_aiming_flag==1)&&(autoaiming_update_flag==1))
			{
				robot_ctrl->yaw_angle_set_energy += autoaiming_ctrl.yaw_delt_angle;//云台角度根据不同算法有不同的结果但是范围是固定的
			}
			else
			{
				robot_ctrl->yaw_angle_set_energy -= rc_ctrl->ch1/200.0f;
			}
			
			//
			if(robot_ctrl->yaw_angle_set_energy<-45.0f)
			{
				robot_ctrl->yaw_angle_set_energy=-45.0f;
			}
			else if(robot_ctrl->yaw_angle_set_energy>45.0f)
			{
				robot_ctrl->yaw_angle_set_energy=45.0f;
			}
		}
		else
		{
			if((robot_ctrl->auto_aiming_flag==1)&&(autoaiming_update_flag==1))
			{
				robot_ctrl->yaw_angle_set += autoaiming_ctrl.yaw_delt_angle;
			}
			else if(rc_ctrl->mouse.vx!=0) //鼠标的左右，以鼠标的前后左右建立的坐标系
			{
				if(rc_ctrl->keyboard.key_bit.CTRL){robot_ctrl->yaw_angle_set -= rc_ctrl->mouse.vx/100.0f;}
				else {robot_ctrl->yaw_angle_set -= rc_ctrl->mouse.vx/20.0f;}
			}
			else
			{
				robot_ctrl->yaw_angle_set -= rc_ctrl->ch1/200.0f;
//				robot_ctrl->yaw_angle_set = 10;
			}
				
			//yaw_limit
			if(rc_ctrl->sw_right_downtomid_flag==1){robot_ctrl->yaw_angle_set=IMU_yaw_angle_dial;} //切换到使能状态，初始化yaw轴角度
			
			if(robot_ctrl->yaw_angle_set>=90.0f)
			{
				robot_ctrl->yaw_angle_set = robot_ctrl->yaw_angle_set-90.0f;
				dial_rotate_angle+=90.0f; if(dial_rotate_angle==360.0f){dial_rotate_angle=0.0f;} //方向反了,互换这里
				IMU_yaw_dial_update();
			}
			else if(robot_ctrl->yaw_angle_set<=-90.0f)
			{
				robot_ctrl->yaw_angle_set = robot_ctrl->yaw_angle_set+90.0f;
				dial_rotate_angle-=90.0f; if(dial_rotate_angle==-90.0f){dial_rotate_angle=270.0f;} //方向反了,互换这里
				IMU_yaw_dial_update();
			}
		}
		
		//pitch
		if((robot_ctrl->auto_aiming_flag==1)&&(autoaiming_update_flag==1))
		{
			robot_ctrl->pitch_angle_set = autoaiming_ctrl.pitch_absolute_angle;
			autoaiming_update_flag = 0;
		}
		else if(rc_ctrl->mouse.vy!=0)
		{
			if(rc_ctrl->keyboard.key_bit.CTRL){robot_ctrl->pitch_angle_set += rc_ctrl->mouse.vy/100.0f;}
			else {robot_ctrl->pitch_angle_set += rc_ctrl->mouse.vy/20.0f;}
		}
		else
		{
			robot_ctrl->pitch_angle_set -= rc_ctrl->ch2/500.0f;
//			robot_ctrl->pitch_angle_set = 10;
		}
		
		//pitch_limit
		if(rc_ctrl->sw_right_downtomid_flag==1){robot_ctrl->pitch_angle_set=0;} //切换到使能状态，初始化pitch轴角度
		
		if(robot_ctrl->pitch_angle_set>GIMBAL_PITCH_MAXANGLE){robot_ctrl->pitch_angle_set = GIMBAL_PITCH_MAXANGLE;}
		else if(robot_ctrl->pitch_angle_set<GIMBAL_PITCH_MINANGLE){robot_ctrl->pitch_angle_set = GIMBAL_PITCH_MINANGLE;}
		
		
		/**********************发射机构控制*******************/
		//
		if((key_shoot_open_flag==1)||(rc_ctrl->sw_left==RC_SW_UP)) //左拨档上，开摩擦轮电机和红点激光
		{
			robot_ctrl->shoot_open_flag = 1;
			
			/**********************拨弹动作控制*******************/
			//
			if((rc_ctrl->wheel>350)||(rc_ctrl->mouse.press_r)||(autoaiming_ctrl.shoot_control==0x02)) //拨轮向下/点击鼠标左键/右键,拨弹发射		
			{
				robot_ctrl->turnbullet_flag = 1;
			}
			else
			{
				robot_ctrl->turnbullet_flag = 0;//拨弹控制信号
			}
			
			//if(((rc_ctrl->keyboard.key_bit.CTRL)&&(rc_ctrl->mouse.press_l))||(autoaiming_ctrl.shoot_control==0x01))
			if(((rc_ctrl->mouse.press_l))||(autoaiming_ctrl.shoot_control==0x01))
			{
				robot_ctrl->turnbullet_once_flag = 1;
				autoaiming_ctrl.shoot_control = 0x00;
			}
			
			//
			if((rc_ctrl->wheel<-350)||(rc_ctrl->keyboard.key_bit.X)) //拨轮向上/常按X,反转退弹
			{
				robot_ctrl->turnbullet_reverse_flag = 1;
			}
			else
			{
				robot_ctrl->turnbullet_reverse_flag = 0;
			}
		}
		else
		{
			robot_ctrl->shoot_open_flag = 0;
			robot_ctrl->turnbullet_flag = 0;
			robot_ctrl->turnbullet_once_flag = 0;
			robot_ctrl->turnbullet_reverse_flag = 0;
		}
	}
}
